#ifndef PID_H
#define PID_H

#include "struct_typedef.h"
#include "stdint.h"
#include "string.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "user_lib.h"
#include "arm_math.h"
#include <math.h>

/********************************** COMMON PID **********************************/
enum PID_MODE
{
    PID_POSITION = 0,
    PID_DELTA
};

typedef struct
{
    uint8_t mode;
    //PID ������
    fp32 Kp;
    fp32 Ki;
    fp32 Kd;

    fp32 max_out;  //������
    fp32 max_iout; //���������

    fp32 set;
    fp32 fdb;

    fp32 out;
    fp32 Pout;
    fp32 Iout;
    fp32 Dout;
    fp32 Dbuf[3];  //΢���� 0���� 1��һ�� 2���ϴ�
    fp32 error[3]; //����� 0���� 1��һ�� 2���ϴ�

} pid_type_def;

/**
  * @brief          pid struct data init
  * @param[out]     pid: PID�ṹ����ָ��
  * @param[in]      mode: PID_POSITION:��ͨPID
  *                 PID_DELTA: ���PID
  * @param[in]      PID: 0: kp, 1: ki, 2:kd
  * @param[in]      max_out: pid������
  * @param[in]      max_iout: pid���������
  * @retval         none
  */
extern void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout);

/**
  * @brief          pid����
  * @param[out]     pid: PID�ṹ����ָ��
  * @param[in]      ref: ��������
  * @param[in]      set: �趨ֵ
  * @retval         pid���
  */
extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set);

/**
  * @brief          pid ������
  * @param[out]     pid: PID�ṹ����ָ��
  * @retval         none
  */
extern void PID_clear(pid_type_def *pid);

extern void PID_change(pid_type_def *pid, fp32 Kp, fp32 Ki, fp32 Kd);

/******************************** FUZZY PID **********************************/
#ifndef abs
#define abs(x) ((x > 0) ? x : -x)
#endif

#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif

#define NB -3
#define NM -2
#define NS -1
#define ZE 0
#define PS 1
#define PM 2
#define PB 3

typedef __packed struct
{
    fp32 KpFuzzy;
    fp32 KiFuzzy;
    fp32 KdFuzzy;

    fp32 (*FuzzyRuleKp)[7];
    fp32 (*FuzzyRuleKi)[7];
    fp32 (*FuzzyRuleKd)[7];

    fp32 KpRatio;
    fp32 KiRatio;
    fp32 KdRatio;

    fp32 eStep;
    fp32 ecStep;

    fp32 e;
    fp32 ec;
    fp32 eLast;

    uint32_t DWT_CNT;
    fp32 dt;
} FuzzyRule_t;

extern void Fuzzy_Rule_Init(FuzzyRule_t *fuzzyRule, fp32 (*fuzzyRuleKp)[7], fp32 (*fuzzyRuleKi)[7], fp32 (*fuzzyRuleKd)[7], fp32 kpRatio, fp32 kiRatio, fp32 kdRatio, fp32 eStep, fp32 ecStep);
extern void Fuzzy_Rule_Implementation(FuzzyRule_t *fuzzyRule, fp32 measure, fp32 ref);

/******************************* PID CONTROL *********************************/
typedef enum pid_Improvement_e
{
    NONE = 0X00,                        //0000 0000
    Integral_Limit = 0x01,              //0000 0001
    Derivative_On_Measurement = 0x02,   //0000 0010
    Trapezoid_Intergral = 0x04,         //0000 0100
    Proportional_On_Measurement = 0x08, //0000 1000
    OutputFilter = 0x10,                //0001 0000
    ChangingIntegrationRate = 0x20,     //0010 0000
    DerivativeFilter = 0x40,            //0100 0000
    ErrorHandle = 0x80,                 //1000 0000
} PID_Improvement_e;

typedef enum errorType_e
{
    PID_ERROR_NONE = 0x00U,
    Motor_Blocked = 0x01U
} ErrorType_e;

typedef __packed struct
{
    uint64_t ERRORCount;
    ErrorType_e ERRORType;
} PID_ErrorHandler_t;

typedef __packed struct pid_t
{
    fp32 Ref;
    fp32 Kp;
    fp32 Ki;
    fp32 Kd;

    fp32 Measure;
    fp32 Last_Measure;
    fp32 Err;
    fp32 Last_Err;
    fp32 Last_ITerm;

    fp32 Pout;
    fp32 Iout;
    fp32 Dout;
    fp32 ITerm;

    fp32 Output;
    fp32 Last_Output;
    fp32 Last_Dout;

    fp32 MaxOut;
    fp32 IntegralLimit;
    fp32 DeadBand;
    fp32 ControlPeriod;
    fp32 CoefA;         //For Changing Integral
    fp32 CoefB;         //ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
    fp32 Output_LPF_RC; // RC = 1/omegac
    fp32 Derivative_LPF_RC;

    uint16_t OLS_Order;
    Ordinary_Least_Squares_t OLS;

    uint32_t DWT_CNT;
    fp32 dt;

    FuzzyRule_t *FuzzyRule;

    uint8_t Improve;

    PID_ErrorHandler_t ERRORHandler;

    void (*User_Func1_f)(struct pid_t *pid);
    void (*User_Func2_f)(struct pid_t *pid);
} PID_t;

void PID_Init
(
    PID_t *pid,
    fp32 max_out,
    fp32 intergral_limit,
    fp32 deadband,

    fp32 kp,
    fp32 ki,
    fp32 kd,

    fp32 A,
    fp32 B,

    fp32 output_lpf_rc,
    fp32 derivative_lpf_rc,

    uint16_t ols_order,

    uint8_t improve
);

extern fp32 PID_Calculate(PID_t *pid, fp32 measure, fp32 ref);

/*************************** FEEDFORWARD CONTROL *****************************/
typedef __packed struct
{
    fp32 c[3]; // G(s) = 1/(c2s^2 + c1s + c0)

    fp32 Ref;
    fp32 Last_Ref;

    fp32 DeadBand;

    uint32_t DWT_CNT;
    fp32 dt;

    fp32 LPF_RC; // RC = 1/omegac

    fp32 Ref_dot;
    fp32 Ref_ddot;
    fp32 Last_Ref_dot;

    uint16_t Ref_dot_OLS_Order;
    Ordinary_Least_Squares_t Ref_dot_OLS;
    uint16_t Ref_ddot_OLS_Order;
    Ordinary_Least_Squares_t Ref_ddot_OLS;

    fp32 Output;
    fp32 MaxOut;

} Feedforward_t;

void Feedforward_Init
(
    Feedforward_t *ffc,
    fp32 max_out,
    fp32 *c,
    fp32 lpf_rc,
    uint16_t ref_dot_ols_order,
    uint16_t ref_ddot_ols_order
);

extern fp32 Feedforward_Calculate(Feedforward_t *ffc, fp32 ref);

/************************* LINEAR DISTURBANCE OBSERVER *************************/
typedef __packed struct
{
    fp32 c[3]; // G(s) = 1/(c2s^2 + c1s + c0)

    fp32 Measure;
    fp32 Last_Measure;

    fp32 u; // system input

    fp32 DeadBand;

    uint32_t DWT_CNT;
    fp32 dt;

    fp32 LPF_RC; // RC = 1/omegac

    fp32 Measure_dot;
    fp32 Measure_ddot;
    fp32 Last_Measure_dot;

    uint16_t Measure_dot_OLS_Order;
    Ordinary_Least_Squares_t Measure_dot_OLS;
    uint16_t Measure_ddot_OLS_Order;
    Ordinary_Least_Squares_t Measure_ddot_OLS;

    fp32 Disturbance;
    fp32 Output;
    fp32 Last_Disturbance;
    fp32 Max_Disturbance;
} LDOB_t;

void LDOB_Init
(
    LDOB_t *ldob,
    fp32 max_d,
    fp32 deadband,
    fp32 *c,
    fp32 lpf_rc,
    uint16_t measure_dot_ols_order,
    uint16_t measure_ddot_ols_order
);

extern fp32 LDOB_Calculate(LDOB_t *ldob, fp32 measure, fp32 u);

/*************************** Tracking Differentiator ***************************/
typedef __packed struct
{
    fp32 Input;

    fp32 h0;
    fp32 r;

    fp32 x;
    fp32 dx;
    fp32 ddx;

    fp32 last_dx;
    fp32 last_ddx;

    uint32_t DWT_CNT;
    fp32 dt;
} TD_t;

extern void TD_Init(TD_t *td, fp32 r, fp32 h0);
extern fp32 TD_Calculate(TD_t *td, fp32 input);

#endif
